#include "camshift_wrapper.h"
#include "x_except.hpp"

#include <iostream>

using namespace cv;



//////////////////////////////////
// releaseTracker()
//
void camshift_wrapper::release_tracker()
{
  started_ = false;
  lost_=true;
}


//////////////////////////////////
// startTracking()
//
void camshift_wrapper::start_tracking(const cv::Mat& img, const cv::Rect& selection)
{
  if(started())
    release_tracker();

  update_hue_image(img);

	float maxVal = 0.f;

  int hsize = 16;
  const float* phranges = rangesArr;

  Mat roi(hueImg, selection);
  Mat maskroi(mask, selection);
  calcHist(&roi, 1, 0, maskroi, hist, 1, &hsize, &phranges);
  normalize(hist, hist, 0, 255, CV_MINMAX);
                
  trackWindow = selection;

  started_ = true;
}

Mat camshift_wrapper::hist_image() const
{
  int hsize = 16;
  Mat histimg = Mat::zeros(200, 320, CV_8UC3);
  histimg  = Scalar::all(0);
  int binW = histimg.cols / hsize;
  Mat buf(1, hsize, CV_8UC3);
  for( int i = 0; i < hsize; i++ )
      buf.at<Vec3b>(i) = Vec3b(saturate_cast<uchar>(i*180./hsize), 255, 255);
  cvtColor(buf, buf, CV_HSV2BGR);
                    
  for( int i = 0; i < hsize; i++ )
  {
      int val = saturate_cast<int>(hist.at<float>(i)*histimg.rows/255);
      rectangle( histimg, Point(i*binW,histimg.rows),
                  Point((i+1)*binW,histimg.rows - val),
                  Scalar(buf.at<Vec3b>(i)), -1, 8 );
  }
  return histimg;
}


//////////////////////////////////
// track()
//
RotatedRect camshift_wrapper::track(const Mat& img)
{
  if(!started_)
    return trackBox;

  chr.start();

	update_hue_image(img);

  const float* phranges = rangesArr;

  calcBackProject(&hueImg, 1, 0, hist, probImg, &phranges);
  probImg &= mask;

  Rect auxWindow = trackWindow;
  RotatedRect auxBox = trackBox;
  auxBox = CamShift(probImg, auxWindow, criteria);

  if( auxWindow.x<=0 || auxWindow.y<=0 || auxWindow.height<=0 || auxWindow.width<=0 ||
      auxBox.center.x<=0 || auxBox.center.y<=0 || auxBox.size.height<=0 || auxBox.size.width<=0)
    lost_=true;
  else
  {
    trackWindow = auxWindow;
    trackBox = auxBox;
    lost_=false;
  }

  chr.stop();
	return trackBox;
}


//////////////////////////////////
// updateHueImage()
//
void camshift_wrapper::update_hue_image(const Mat& img)
{  
  cvtColor(img, hsvImg, CV_BGR2HSV);
  
  inRange(hsvImg, Scalar(0, smin, MIN(vmin,vmax)),
                    Scalar(180, 256, MAX(vmin, vmax)), mask);
  
  int ch[] = {0, 0};
  hueImg.create(hsvImg.size(), hsvImg.depth());
  mixChannels(&hsvImg, 1, &hueImg, 1, ch, 1);
}



